#include "../include/BlockTilting.hpp"

void BlockTilting::init_task(bool is_ext_param)
{   
//Before you browse the code , you had better know the tips below:
//tips: the block tilting system is defined in the photo coordinate_system.png in /data/photos.
//set state
    if(!is_ext_param){
        block_edge  =   0.08;
        block_mass  =   0.5;
        hand_mass    =   0.3; 
        n_min = 10;//normal force
        goal_vel = 0.5; //block velocity
        u_hc = 0.8;
        u_tc = 0.8;
        num_seeds = 3; 
    }
    _dim.Actualized = 3;
    _dim.UnActualized = 6;
    _dim.SlidingFriction = 0;
    _dim.Lambda = 9;
    _dim.Generalized = _dim.Actualized + _dim.UnActualized;

    p_h0_world      <<  block_edge*0.4, 0, block_edge;
    p_h_world = p_h0_world;
    p_obj0_world    <<  block_edge*0.5, 0,          0;
    p_obj_world = p_obj0_world;                                          
    qtn_obj_world    = Quaterniond(1,0,0,0);
    R_obj_world = quat2SO3(qtn_obj_world);

    // q_sys_state << p_obj0_world, quat2vec4(qtn_obj_world), p_h0_world;
    // q_sys_state.head(3) = p_obj0_world;

    E_qo <<     -qtn_obj_world.x(), -qtn_obj_world.y(), -qtn_obj_world.z(),
                qtn_obj_world.w(), -qtn_obj_world.z(),  qtn_obj_world.y(),
                qtn_obj_world.z(),  qtn_obj_world.w(), -qtn_obj_world.x(),
                -qtn_obj_world.y(),  qtn_obj_world.x(),  qtn_obj_world.w();
    E_qo = 0.5*E_qo;
                        
    Omega_map_v2qd = Mat<10,9>::Zero();
    Omega_map_v2qd.block<3,3>(0,0) = R_obj_world;
    Omega_map_v2qd.block<4,3>(3,3) = E_qo;
    Omega_map_v2qd.block<3,3>(7,6) = Mat<3,3>::Identity();

//set goal
    // goal_vel = 0.5; 
    //take care
    tilt_direction << 1, 0, 0;
    sel_matrix = Matrix3::Zero();
    sel_matrix.diagonal()<<1, 0, 1;
    p_tc_world = p_obj0_world;

    rotate_axis = Vec<3>(0,0,1).cross(tilt_direction);
    v_obj_world = -goal_vel*(rotate_axis.cross(p_tc_world));
    w_obj_world =  goal_vel*rotate_axis;
    tw_obj_goal_world = Vector6::Zero();
    tw_obj_goal_world.head(3) = v_obj_world;
    tw_obj_goal_world.tail(3) = w_obj_world;
    
    adjoint_goal_inv = Matrix6::Zero();
    adjoint_goal_inv.block<3,3>(0,0) = R_obj_world.transpose();
    adjoint_goal_inv.block<3,3>(0,3) = -R_obj_world.transpose()*wedge(p_obj_world);
    adjoint_goal_inv.block<3,3>(3,3) = R_obj_world.transpose();

    tw_obj_obj = adjoint_goal_inv * tw_obj_goal_world;
    // Gv = bG
    bG_goal = tw_obj_obj;

    G_goal = Mat<6,9>::Zero();
    G_goal.block<6,6>(0,0) = Matrix6::Identity();
    /*  natural constraints   */
    _gravity    =   9.8;
    F_Go << 0, 0, -block_mass*_gravity;
    F_Gh << 0, 0, -hand_mass*_gravity;
    F_world << R_obj_world.transpose()*F_Go, Vec<3>::Zero(), F_Gh; 

    p_tc1_obj   = rotate_axis*block_edge/2;
    p_tc2_obj   = -rotate_axis*block_edge/2;
    p_tc1_world = p_obj_world + R_obj_world*p_tc1_obj;
    p_tc2_world = p_obj_world + R_obj_world*p_tc2_obj;

    p_hc_obj = p_h0_world - p_obj0_world;

    /*  guard conditions    */      
    Z_unit << 0, 0, 1;
    // u_hc = 0.8;
    // u_tc = 0.8;
    fric_coneside = 6;

    _A = Mat<6*3+1, 9+9>::Zero();

//  init and reset force controller
    _controller->init_controller();

    _controller->reset();

    run_first_task = true;
    is_end_task = false;
    is_stop_moving = false;
}

void BlockTilting::end_task(){
    double _pos[7];
    double _pose_set[7];
    _controller->_hw->getPose(_pos);
    copyArray(_pos, _pose_set, 7);
    _pose_set[0] += 0.05; 
    _controller->ExecuteLinearPos(
        _pose_set, 
        _controller->_fHz,
        _controller->_TimeStepSec);
    is_stop_moving = true;
}

void BlockTilting::run_task()
{   
    double _pos[7]={0};
    double _pose[7]={0};
    if(!run_first_task){
        _controller->_hw->getPose(_pos);
        _controller->_hw->_robot->getObj2World(_pose);
        p_obj_world =sel_matrix * Vector3(_pose[0],_pose[1],_pose[2]);  // obj 2 world not change
        qtn_obj_world = Quaterniond(_pose[3],_pose[4],_pose[5],_pose[6]);
        R_obj_world = quat2SO3(qtn_obj_world);
        p_h_world = sel_matrix * Vector3(_pos[0], _pos[1], _pos[2]);
        p_hc_obj = -p_obj_world + R_obj_world.transpose()*p_h_world;
        p_tc1_world = p_obj_world + R_obj_world*p_tc1_obj;
        p_tc2_world = p_obj_world + R_obj_world*p_tc2_obj;
        F_world << R_obj_world.transpose()*F_Go, Vec<3>::Zero(), F_Gh; 

        adjoint_goal_inv = Matrix6::Zero();
        adjoint_goal_inv.block<3,3>(0,0) = R_obj_world.transpose();
        adjoint_goal_inv.block<3,3>(0,3) = -R_obj_world.transpose()*wedge(p_obj_world);
        adjoint_goal_inv.block<3,3>(3,3) = R_obj_world.transpose();

        bG_goal = adjoint_goal_inv * tw_obj_goal_world;

        Eigen::Quaterniond  q1 = Quaterniond(1,0,0,0);
        Eigen::Quaterniond  q2 = qtn_obj_world;

        //calculate the minimum angle between 2 quaternions
        double dot = q1.normalized().dot(q2.normalized());
        double cos_value = 2.0*dot*dot - 1.0;
        double ang;
        if (cos_value > 0.999999) ang = 0;
        else if (cos_value < -0.999999) ang = PI;
        else ang = acos(cos_value);
        if (ang > PI){
        ang = 2.0*PI - ang;
        }
        printf("fabs ang %.4f\n", fabs(ang));

        if(fabs(ang) > 0.81) // 0.84 * 180 / 3.14 = 48.15 deg
            is_end_task = true;
    }else{
            _controller->_hw->getPose(_pos);
            // _pos[0]=p_h0_world(0);
            // _pos[1]=p_h0_world(1);
            // _pos[2]=p_h0_world(2);
            // _pos[3]=0;
            // _pos[4]=-1;
            // _pos[5]=0;
            // _pos[6]=0;

        run_first_task = false; 
    }
    
    tw_obj_obj = adjoint_goal_inv * tw_obj_goal_world;

    E_qo << -qtn_obj_world.x(), -qtn_obj_world.y(), -qtn_obj_world.z(),
            qtn_obj_world.w(), -qtn_obj_world.z(),  qtn_obj_world.y(),
            qtn_obj_world.z(),  qtn_obj_world.w(), -qtn_obj_world.x(),
            -qtn_obj_world.y(),  qtn_obj_world.x(),  qtn_obj_world.w();
    E_qo = 0.5*E_qo;
                    
    Omega_map_v2qd = Mat<10,9>::Zero();
    Omega_map_v2qd.block<3,3>(0,0) = R_obj_world;
    Omega_map_v2qd.block<4,3>(3,3) = E_qo;
    Omega_map_v2qd.block<3,3>(7,6) = Mat<3,3>::Identity();

    jac_phi_q = cal_jac_phi_q(p_obj_world, quat2vec4(qtn_obj_world), p_h_world, p_hc_obj, p_tc1_world, p_tc2_world, p_tc1_obj, p_tc2_obj);

    for(auto i=0; i<fric_coneside; i++)
    {
        friction_dir[i](0,0)=sin(2*PI*(i+1)/fric_coneside);
        friction_dir[i](1,0)=cos(2*PI*(i+1)/fric_coneside);
        friction_dir[i](2,0)=0;       
    }
    //fric_coneside *3 + 1 , _dim.Generalized + _dim.Lambda 

    for(auto i = 0; i<fric_coneside; i++)
    {
        _A.block<1,3>(i, 0) = (friction_dir[i].transpose()-u_hc*Z_unit.transpose())*R_obj_world.transpose();
        _A.block<1,3>(i+fric_coneside, 3) = friction_dir[i].transpose()-u_tc*Z_unit.transpose();
        _A.block<1,3>(i+2*fric_coneside, 6) = friction_dir[i].transpose()-u_tc*Z_unit.transpose();
    }
    _A.block<1,3>(6*3,0) = -Z_unit.transpose()*R_obj_world.transpose();

    _b_A = Vec<6*3+1>::Zero();
    _b_A(6*3) = -n_min;

    // num_seeds = 3;  
    print_level = 0;

    solvehfvc(  
            jac_phi_q*Omega_map_v2qd,
            G_goal, 
            bG_goal,
            F_world,
            _Aeq, 
            _b_Aeq,
            _A, 
            _b_A,
            _dim.Actualized,        
            _dim.UnActualized,
            _dim.SlidingFriction,  
            _dim.Lambda,
            num_seeds, 
            print_level,
            &_action);

    cal_hand_cmd(&_action, &_hand);

    // cout << "force_cmd_world" << endl << _hand.force_cmd_world << endl;
    // cout << "vel_cmd_world" << endl << _hand.vel_cmd_world << endl;

    double pose_set[7] = {0};
    _controller->_hw->getPose(pose_set); // get quaternion
    pose_set[0] = pose_set[0]+ _hand.vel_cmd_world[0]*_controller->_TimeStepSec;
    pose_set[2] = pose_set[2]+ _hand.vel_cmd_world[2]*_controller->_TimeStepSec;

    if(_print_flag){
        Clock::time_point timenow_clock = Clock::now();
        double timenow = double(std::chrono::duration_cast<std::chrono::nanoseconds>(timenow_clock - _time0).count())/1e6; // milli second
        _file << timenow << " ";
        stream_array_in(_file, pose_set, 3);
        stream_array_in3d(_file, _hand.vel_cmd_world);
        stream_array_in3d(_file, _hand.force_cmd_world);
        _file << std::endl;
    }

    double force_set[6] = {0};
    for (int i = 0; i < _action.n_af; ++i)  
        force_set[i] = _action.eta_af(i);

    int n_af = _action.n_af;//2
    int n_av = _action.n_av + 3;//1+3

    Matrix6 T_w = Matrix6::Zero();
    T_w.block<3, 3>(0, 0) = _action.R_a;
    T_w(3, 3) = 1;
    T_w(4, 4) = 1;
    T_w(5, 5) = 1;  

    Matrix4 SE3_WT_fb = posem2SE3(_pos);
    Matrix6 Adj_WT = SE32Adj(SE3_WT_fb);
    Matrix6 T_T = T_w * Adj_WT;

    _controller->ExecuteHFVC(
        n_af,n_av,
        T_T, 
        pose_set,
        force_set,
        HYBRID_SERVO_MODE::HS_CONTINUOUS,
        _controller->_fHz,
        _controller->_TimeStepSec
    );
}

//refence in block tilting demo : Derive jacobian function for 'block tilting' example
Mat<9,10> BlockTilting::cal_jac_phi_q(Vec<3> &p_wo, Vec<4> &q_wo, Vec<3> &p_wh, Vec<3> &p_ohc, Vec<3> &p_wtc_1, Vec<3> &p_wtc_2, Vec<3> &p_otc1, Vec<3> &p_otc2)
{
    Mat<9,10> jac;
    double t2 = q_wo(0)*q_wo(0);
    double t3 = q_wo(1)*q_wo(1);
    double t4 = q_wo(2)*q_wo(2);
    double t5 = q_wo(3)*q_wo(3);
    double t6 = p_otc1(0)*q_wo(0)*2.0;
    double t7 = p_otc1(0)*q_wo(1)*2.0;
    double t8 = p_otc2(0)*q_wo(0)*2.0;
    double t9 = p_otc1(0)*q_wo(2)*2.0;
    double t10 = p_otc2(0)*q_wo(1)*2.0;
    double t11 = p_otc1(0)*q_wo(3)*2.0;
    double t12 = p_otc2(0)*q_wo(2)*2.0;
    double t13 = p_otc2(0)*q_wo(3)*2.0;
    double t14 = p_otc1(1)*q_wo(0)*2.0;
    double t15 = p_otc1(1)*q_wo(1)*2.0;
    double t16 = p_otc2(1)*q_wo(0)*2.0;
    double t17 = p_otc1(1)*q_wo(2)*2.0;
    double t18 = p_otc2(1)*q_wo(1)*2.0;
    double t19 = p_otc1(1)*q_wo(3)*2.0;
    double t20 = p_otc2(1)*q_wo(2)*2.0;
    double t21 = p_otc2(1)*q_wo(3)*2.0;
    double t22 = p_otc1(2)*q_wo(0)*2.0;
    double t23 = p_otc1(2)*q_wo(1)*2.0;
    double t24 = p_otc2(2)*q_wo(0)*2.0;
    double t25 = p_otc1(2)*q_wo(2)*2.0;
    double t26 = p_otc2(2)*q_wo(1)*2.0;
    double t27 = p_otc1(2)*q_wo(3)*2.0;
    double t28 = p_otc2(2)*q_wo(2)*2.0;
    double t29 = p_otc2(2)*q_wo(3)*2.0;
    double t30 = p_wh(0)*q_wo(0)*2.0;
    double t31 = p_wh(0)*q_wo(1)*2.0;
    double t32 = p_wh(1)*q_wo(0)*2.0;
    double t33 = p_wh(0)*q_wo(2)*2.0;
    double t34 = p_wh(1)*q_wo(1)*2.0;
    double t35 = p_wh(2)*q_wo(0)*2.0;
    double t36 = p_wh(0)*q_wo(3)*2.0;
    double t37 = p_wh(1)*q_wo(2)*2.0;
    double t38 = p_wh(2)*q_wo(1)*2.0;
    double t39 = p_wh(1)*q_wo(3)*2.0;
    double t40 = p_wh(2)*q_wo(2)*2.0;
    double t41 = p_wh(2)*q_wo(3)*2.0;
    double t42 = q_wo(0)*q_wo(1)*2.0;
    double t43 = q_wo(0)*q_wo(2)*2.0;
    double t44 = q_wo(0)*q_wo(3)*2.0;
    double t45 = q_wo(1)*q_wo(2)*2.0;
    double t46 = q_wo(1)*q_wo(3)*2.0;
    double t47 = q_wo(2)*q_wo(3)*2.0;
    double t48 = -t9;
    double t49 = -t12;
    double t50 = -t19;
    double t51 = -t21;
    double t52 = -t23;
    double t53 = -t26;
    double t54 = -t34;
    double t55 = -t36;
    double t56 = -t40;
    double t57 = -t3;
    double t58 = -t4;
    double t59 = -t5;
    double t60 = t7+t17+t27;
    double t61 = t10+t20+t29;
    double t62 = t31+t37+t41;
    double t63 = t15+t22+t48;
    double t64 = t11+t14+t52;
    double t65 = t6+t25+t50;
    double t66 = t18+t24+t49;
    double t67 = t13+t16+t53;
    double t68 = t8+t28+t51;
    double t69 = t33+t35+t54;
    double t70 = t32+t38+t55;
    double t71 = t30+t39+t56;

    jac <<  -1,  0, 0,  t71,    t62,            -t33+t34-t35,       t70,                t2+t3+t58+t59,          t44+t45,        -t43+t46,
             0, -1, 0,  t70,    t69,            t62,                -t30-t39+t40,       -t44+t45,               t2+t4+t57+t59,  t42+t47,
             0,  0,-1,  t69,    -t32+t36-t38,   t71,                t62,                t43+t46,                -t42+t47,       t2+t5+t57+t58,
             1,  0, 0,  t65,    t60,            t63,                -t11-t14+t23,       0.0,                    0.0,            0.0,
             0,  1, 0,  t64,    t9-t15-t22,     t60,                t65,                0.0,                    0.0,            0.0,
             0,  0, 1,  t63,    t64,            -t6+t19-t25,        t60,                0.0,                    0.0,            0.0,
             1,  0, 0,  t68,    t61,            t66,                -t13-t16+t26,       0.0,                    0.0,            0.0,
             0,  1, 0,  t67,    t12-t18-t24,    t61,                t68,                0.0,                    0.0,            0.0,
             0,  0, 1,  t66,    t67,            -t8+t21-t28,        t61,                0.0,                    0.0,            0.0;
    return jac;
}

void BlockTilting::cal_hand_cmd(HFVCDataTypedef *_hfvc, RbHandDataTypedef *_hand)
{
    VectorXd v_supplement;
    VectorXd f_supplement;
    v_supplement.resize(_hfvc->n_af);
    f_supplement.resize(_hfvc->n_av);
    
    v_supplement.setZero();
    f_supplement.setZero();

    VectorXd vel_cols(v_supplement.rows()+_hfvc->w_av.rows(), 1);
    VectorXd force_cols(_hfvc->eta_af.rows()+f_supplement.rows(), 1);

    vel_cols << v_supplement, _hfvc->w_av;
    force_cols << _hfvc->eta_af, f_supplement;
// World frame velocity:
    _hand->vel_cmd_world = _hfvc->R_a.inverse()*vel_cols;
// World frame force:
    _hand->force_cmd_world = _hfvc->R_a.inverse()*force_cols;
}

void BlockTilting::init(ForceControlController *_ctrl,   std::chrono::high_resolution_clock::time_point _time){
    _controller=_ctrl;

    _time0 = _time;
    if (_print_flag) {
        _file.open(fullpath);
        if (_file.is_open())
            printf("[BlockTilting] file opened successfully.\n");
        else
            printf("[BlockTilting] Failed to open file.\n");
    }
}

BlockTilting::BlockTilting()
{
    run_first_task = true;
    _hand.init();
    _dim.init();
}
BlockTilting::~BlockTilting()
{
}



